package nav;

import tools.*;
import lejos.util.TimerListener;

/**
 * uses odometer readings and target Position to compute scaling factors for
 * forward and rotation Speed meant to be used with the NavigatorClass only
 * 
 * @author Antoine Lennartz
 */
public class SpeedController implements TimerListener {
	/** lower bound for speed scales */
	public static final double MIN_RTTN_SCALE = 0.25, MIN_FRWRD_SCALE = 0.14;
	
	/** link to odometer */
	private Odometer odo;
	
	/** target position */
	private double[] targPos = new double[3];
	
	/** unit heading vector */
	private double[] unitH = new double[2];
	
	/** target vector */
	private double[] targV = new double[2];
	
	/** current position */
	private double[] cPos = new double[3];
	
	/** scale for forward Speed */
	private double forwardScale;
	
	/** scale for rotation Speed */
	private double rotationScale;
	
	/** Lock for vectors */
	private Object vectorLock = new Object();
	
	/** Lock for scales */
	private Object scaleLock = new Object();
	
	/** Enables and disables the speed controller */
	private boolean enabled = false;
	/**
	 * used to determine whether or not the robot is allowed to drive backwards
	 * when this is set to false the robot will turn on its centre until it is
	 * less than 'minAngle' degrees away from its target heading
	 */
	private boolean allowReverse = false;
	/** minimal angle */
	private int minAngle = 45;
	/**
	 * timer for SpeedController. Every time it ticks the controller updates the
	 * unit heading vector and the target vector
	 */
	private Timer2 t;

	/** constructor */
	public SpeedController(Odometer odo) {
		this.odo = odo;
		t = new Timer2(Odometer.DEFAULT_PERIOD, this);
		t.start();
	}

	@Override
	public void timedOut() {
		if (enabled) {
			updateInfo();
		}
	}

	/**
	 * gets called by timedOut() updates the relevant vectors from current
	 * odometry position
	 */
	private void updateInfo() {
		// update cPos
		odo.getPosition(cPos);
		synchronized (vectorLock) {
			// compute unitH
			unitH[0] = Math.sin(cPos[2]);
			unitH[1] = Math.cos(cPos[2]);
			// compute targV
			targV[0] = targPos[0] - cPos[0];
			targV[1] = targPos[1] - cPos[1];
		}

	}

	/** computes speed scales*/
	private void computeScales() {
		double dTheta = getdTheta(), dDist=getDist();		
		synchronized (scaleLock) {
			//default scales for reaching target point
			forwardScale = Math.cos(dTheta);
			rotationScale = Math.sin(dTheta);
			
			//make the robot slow down as it approaches its destination
			if(dDist<5) forwardScale*=dDist/5;
			//if(dTheta<Fast.toRad(10))rotationScale*=dTheta/Fast.toRad(10);
			
			/*makes sure the robot does not slow down too much */
			if(Math.abs(forwardScale)< MIN_FRWRD_SCALE)
				forwardScale = MIN_FRWRD_SCALE * Fast.signOf(forwardScale);
			if(Math.abs(rotationScale)< MIN_RTTN_SCALE)
				rotationScale = MIN_RTTN_SCALE * Fast.signOf(rotationScale);
			
			/*if the robot is facing more than minAngle away from its target
			 *and is not allowed to reverse*/
			if(Math.abs(dTheta) > minAngle && !allowReverse) {
				//rotate before moving forwards
				forwardScale = 0;
				rotationScale=Fast.signOf(rotationScale);
			} 
			
			/*if the robot has reached the target position set the rotation scale 
			 * to depend on the final angle*/
			if(dDist<1) rotationScale=Math.sin(getdAng());
		}
	}

	/** compute speed scales for rotating the robot */
	/*private void computeAngleScales() {
		double theta = getdAng();
		synchronized (scaleLock) {
			forwardScale = 0;
			rotationScale = Math.sin(theta);
			if (Math.abs(theta) > 90)
				rotationScale /= Math.abs(rotationScale);
			else if (Math.abs(rotationScale) < MIN_RTTN_SCALE
					&& Math.abs(theta) > 2)
				rotationScale *= MIN_RTTN_SCALE / Math.abs(rotationScale);
		}

	}*/

	// accessors
	/**
	 * accessor for pointScales
	 * 
	 * @param scales
	 *            - buffer to write scales into
	 */
	public void getScales(double[] scales) {
		computeScales();
		synchronized (scaleLock) {
			scales[0] = forwardScale;
			scales[1] = rotationScale;
		}

	}

	/**
	 * accessor for angleScales
	 * 
	 * @param scales
	 *            - buffer to write scales into
	 */
	/*public void getAngleScales(double[] scales) {
		computeAngleScales();
		synchronized (scaleLock) {
			scales[0] = forwardScale;
			scales[1] = rotationScale;
		}

	}*/
	
	/** @return dTheta in radians: the angle between the current heading vector and the wanted
	 * heading vector */
	public double getdTheta() {
		synchronized (vectorLock) {
			return Fast.fixAngle2(Math.atan2(targV[1], targV[0]) - cPos[2]);
		}
	}

	/** @return distance to target */
	public double getDist() {
		synchronized (vectorLock) {
			return Fast.magnitude(targV);
		}
	}

	/**
	 * @return the amount of degrees between the current and final target heading.
	 *         Formated: [-PI, PI]
	 */
	public double getdAng() {
		synchronized (vectorLock) {
			return Fast.fixAngle2(targPos[2] - cPos[2]);
		}
	}

	// mutators
	/** sets target position */
	public void setTargPos(double[] targPos) {
		this.targPos[0] = targPos[0];
		this.targPos[1] = targPos[1];
		//this.targPos[2] = targPos[2];
	}

	/** enables or disables the speedController */
	public void setEnabled(boolean enabled) {
		this.enabled = enabled;
		updateInfo();
	}

	/**
	 * sets the boolean which decides whether or not the robot is allowed to
	 * drive backwards to reach its destination rather than turn around and go
	 * forward
	 * 
	 * @param allowReverse
	 *            - new value for boolean
	 */
	public void setAllowReverse(boolean allowReverse) {
		this.allowReverse = allowReverse;
	}

}
